clc;
clear;

%% main主脚本
tic;
startTime = cputime;
pde = process_pde_NS_t(1);
process_plot_ex_scalar(pde, pde.p, pde.domain(8));
vector_h = [1/2 1/4 1/6 1/8];
basis_type1 = "P2";
basis_type2 = "P1";

u_Linf = zeros(size(vector_h,2),1);
u_L2 = zeros(size(vector_h,2),1);
u_H1 = zeros(size(vector_h,2),1);
p_Linf = zeros(size(vector_h,2),1);
p_L2 = zeros(size(vector_h,2),1);
p_H1 = zeros(size(vector_h,2),1);

for i = 1:size(vector_h,2)
    h = vector_h(i);
    dt = h^2;
    Nt = (pde.domain(8) - pde.domain(7))/dt;

    mesh = process_mesh(pde, h);
    fs1 = process_fespace(mesh, basis_type1);
    fs2 = process_fespace(mesh, basis_type2);

    A1 = pde.nu*term_grad_U_grad_V(mesh, fs1);
    A2 = term_P_div_V(mesh, fs1, fs2);

    O1 = sparse(fs2.Nb, fs2.Nb);
    O2 = sparse(fs2.Nb, 1);

    M = term_U_V(mesh, fs1);

    A = [1/dt*M+A1, -A2;
        -A2', O1];

    U_old = [pde.u1(fs1.node(:,1),fs1.node(:,2),fs1.node(:,3),0);
        pde.u2(fs1.node(:,1),fs1.node(:,2),fs1.node(:,3),0);
        pde.u3(fs1.node(:,1),fs1.node(:,2),fs1.node(:,3),0)];

    for m = 0:Nt-1
        t = (m+1)*dt;
        AN = term_Uold_grad_U_V(mesh, fs1, U_old);

        At = A+blkdiag(AN,O1);
        bt = [1/dt*M*U_old+term_F_V_t(mesh, fs1, pde.f1, pde.f2, pde.f3, t); O2];

        [At, bt] = process_boundary_t(At, bt, mesh, fs1, fs2, pde, t);

        SOL = At\bt;
        U_old = SOL(1:3*fs1.Nb);
    end

    process_plot_scalar(mesh, fs2, SOL(3*fs1.Nb+1:end));

    [u_Linf(i,1), p_Linf(i,1)] = process_error_t(mesh, fs1, fs2, pde, SOL, "Linf");
    [u_L2(i,1), p_L2(i,1)] = process_error_t(mesh, fs1, fs2, pde, SOL, "L2");
    [u_H1(i,1), p_H1(i,1)] = process_error_t(mesh, fs1, fs2, pde, SOL, "H1");

    fprintf("已完成\t%d/%d\n", i, size(vector_h,2));
end

process_rate(u_Linf, vector_h);
process_rate(u_L2, vector_h);
process_rate(u_H1, vector_h);
process_rate(p_Linf, vector_h);
process_rate(p_L2, vector_h);
process_rate(p_H1, vector_h);

TOCtime = toc;
CPUtime = cputime - startTime;
disp(["TOC time(s)", num2str(TOCtime)]);
disp(["CPU time(s)", num2str(CPUtime)]);



%% process_pde_NS
function pde = process_pde_NS_t(index)
switch index
    case 1
        pde.index = index;
        nu = 1;
        pde.nu = nu;
        pde.u1 = @(x,y,z,t)sin(x).^2.*(sin(2*y).*sin(z).^2-sin(y).^2.*sin(2*z)).*cos(t);
        pde.u2 = @(x,y,z,t)sin(y).^2.*(sin(x).^2.*sin(2*z)-sin(2*x).*sin(z).^2).*cos(t);
        pde.u3 = @(x,y,z,t)sin(z).^2.*(sin(2*x).*sin(y).^2-sin(x).^2.*sin(2*y)).*cos(t);
        pde.p = @(x,y,z,t)cos(x).*sin(y).*cos(z).*cos(t);

        pde.u1dx = @(x,y,z,t)cos(t).*cos(x).*sin(x).*(sin(y.*2.0).*sin(z).^2-sin(z.*2.0).*sin(y).^2).*2.0;
        pde.u1dy = @(x,y,z,t)cos(t).*sin(x).^2.*(cos(y.*2.0)-cos(y.*2.0-z.*2.0));
        pde.u1dz = @(x,y,z,t)-cos(t).*sin(x).^2.*(cos(z.*2.0)-cos(y.*2.0-z.*2.0));

        pde.u2dx = @(x,y,z,t)-cos(t).*sin(y).^2.*(cos(x.*2.0)-cos(x.*2.0-z.*2.0));
        pde.u2dy = @(x,y,z,t)cos(t).*cos(y).*sin(y).*(sin(x.*2.0).*sin(z).^2-sin(z.*2.0).*sin(x).^2).*-2.0;
        pde.u2dz = @(x,y,z,t)cos(t).*sin(y).^2.*(cos(z.*2.0)-cos(x.*2.0-z.*2.0));

        pde.u3dx = @(x,y,z,t)cos(t).*sin(z).^2.*(cos(x.*2.0)-cos(x.*2.0-y.*2.0));
        pde.u3dy = @(x,y,z,t)-cos(t).*sin(z).^2.*(cos(y.*2.0)-cos(x.*2.0-y.*2.0));
        pde.u3dz = @(x,y,z,t)cos(t).*cos(z).*sin(z).*(sin(x.*2.0).*sin(y).^2-sin(y.*2.0).*sin(x).^2).*2.0;

        pde.pdx = @(x,y,z,t)-cos(t).*cos(z).*sin(x).*sin(y);
        pde.pdy = @(x,y,z,t)cos(t).*cos(x).*cos(y).*cos(z);
        pde.pdz = @(x,y,z,t)-cos(t).*cos(x).*sin(y).*sin(z);

        pde.f1 = @(x,y,z,t)nu.*cos(t).*(sin(y.*2.0).*cos(x).^2.*sin(z).^2+sin(y.*2.0).*cos(z).^2.*sin(x).^2-sin(z.*2.0).*cos(x).^2.*sin(y).^2-sin(z.*2.0).*cos(y).^2.*sin(x).^2-sin(y.*2.0).*sin(x).^2.*sin(z).^2.*4.0+sin(z.*2.0).*sin(x).^2.*sin(y).^2.*4.0).*-2.0-sin(t).*sin(x).^2.*(sin(y.*2.0).*sin(z).^2-sin(z.*2.0).*sin(y).^2)+cos(t).^2.*cos(x).*sin(x).^3.*(sin(y.*2.0).*sin(z).^2-sin(z.*2.0).*sin(y).^2).^2.*2.0-cos(t).*cos(z).*sin(x).*sin(y)-cos(t).^2.*sin(x).^2.*sin(y).^2.*(sin(x.*2.0).*sin(z).^2-sin(z.*2.0).*sin(x).^2).*(cos(y.*2.0)-cos(y.*2.0-z.*2.0))-cos(t).^2.*sin(x).^2.*sin(z).^2.*(sin(x.*2.0).*sin(y).^2-sin(y.*2.0).*sin(x).^2).*(cos(z.*2.0)-cos(y.*2.0-z.*2.0));
        pde.f2 = @(x,y,z,t)nu.*cos(t).*(sin(x.*2.0).*cos(y).^2.*sin(z).^2+sin(x.*2.0).*cos(z).^2.*sin(y).^2-sin(z.*2.0).*cos(x).^2.*sin(y).^2-sin(z.*2.0).*cos(y).^2.*sin(x).^2-sin(x.*2.0).*sin(y).^2.*sin(z).^2.*4.0+sin(z.*2.0).*sin(x).^2.*sin(y).^2.*4.0).*2.0+sin(t).*sin(y).^2.*(sin(x.*2.0).*sin(z).^2-sin(z.*2.0).*sin(x).^2)+cos(t).^2.*cos(y).*sin(y).^3.*(sin(x.*2.0).*sin(z).^2-sin(z.*2.0).*sin(x).^2).^2.*2.0+cos(t).*cos(x).*cos(y).*cos(z)-cos(t).^2.*sin(x).^2.*sin(y).^2.*(sin(y.*2.0).*sin(z).^2-sin(z.*2.0).*sin(y).^2).*(cos(x.*2.0)-cos(x.*2.0-z.*2.0))+cos(t).^2.*sin(y).^2.*sin(z).^2.*(sin(x.*2.0).*sin(y).^2-sin(y.*2.0).*sin(x).^2).*(cos(z.*2.0)-cos(x.*2.0-z.*2.0));
        pde.f3 = @(x,y,z,t)nu.*cos(t).*(sin(x.*2.0).*cos(y).^2.*sin(z).^2+sin(x.*2.0).*cos(z).^2.*sin(y).^2-sin(y.*2.0).*cos(x).^2.*sin(z).^2-sin(y.*2.0).*cos(z).^2.*sin(x).^2-sin(x.*2.0).*sin(y).^2.*sin(z).^2.*4.0+sin(y.*2.0).*sin(x).^2.*sin(z).^2.*4.0).*-2.0-sin(t).*sin(z).^2.*(sin(x.*2.0).*sin(y).^2-sin(y.*2.0).*sin(x).^2)+cos(t).^2.*cos(z).*sin(z).^3.*(sin(x.*2.0).*sin(y).^2-sin(y.*2.0).*sin(x).^2).^2.*2.0-cos(t).*cos(x).*sin(y).*sin(z)+cos(t).^2.*sin(x).^2.*sin(z).^2.*(sin(y.*2.0).*sin(z).^2-sin(z.*2.0).*sin(y).^2).*(cos(x.*2.0)-cos(x.*2.0-y.*2.0))+cos(t).^2.*sin(y).^2.*sin(z).^2.*(sin(x.*2.0).*sin(z).^2-sin(z.*2.0).*sin(x).^2).*(cos(y.*2.0)-cos(x.*2.0-y.*2.0));

        pde.domain = [0 1 0 1 0 1 0 1];
        pde.bdary_u = [0 0 0 0 0 0];
    otherwise
        error("Invalid pde index.");
end
fprintf("pde index: %d\n", pde.index);
fprintf("pde domain: xmin,xmax,ymin,ymax,zmin,zmax\n");
disp(pde.domain);
if isfield(pde, "bdary_u")
    fprintf("pde boundary: left,front,right,back,bottom,top (0=Dirichlet,1=Neumann)\n");
    disp(pde.bdary_u);
end
if isfield(pde, "bdary_p")
    fprintf("pde boundary of P: (0=Dirichlet,1=Neumann)\n");
    disp(pde.bdary_p);
end
end

%% term_grad_U_grad_V
function matrix_A = term_grad_U_grad_V(mesh, fespace)
A1 = term_grad_P_grad_Q(mesh, fespace);
matrix_A = blkdiag(A1, A1, A1);
end

%% term_P_div_V
function matrix_A = term_P_div_V(mesh, fespace1, fespace2)
Nb1 = fespace1.Nb;
Nlb1 = fespace1.Nlb;
Nb2 = fespace2.Nb;
Nlb2 = fespace2.Nlb;
rows = zeros(mesh.Ne*Nlb1*Nlb2,1);
cols = rows;
val1 = rows;
val2 = rows;
val3 = rows;
flag = 1;
for n = 1:mesh.Ne
    V = mesh.node(mesh.elem(n,:),:);
    [int_points_loc, int_weights_loc] = integral_loc(V, "3d");
    phix = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dx");
    phiy = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dy");
    phiz = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dz");
    psi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "x");
    for i = 1:Nlb1
        for j = 1:Nlb2
            f1 = psi(:,j) .* phix(:,i);
            f2 = psi(:,j) .* phiy(:,i);
            f3 = psi(:,j) .* phiz(:,i);
            r1 = int_weights_loc * f1;
            r2 = int_weights_loc * f2;
            r3 = int_weights_loc * f3;
            rows(flag) = fespace1.elem(n,i);
            cols(flag) = fespace2.elem(n,j);
            val1(flag) = r1;
            val2(flag) = r2;
            val3(flag) = r3;
            flag = flag + 1;
        end
    end
end
A1 = sparse(rows, cols, val1, Nb1, Nb2);
A2 = sparse(rows, cols, val2, Nb1, Nb2);
A3 = sparse(rows, cols, val3, Nb1, Nb2);
matrix_A = [A1; A2; A3];
end

%% term_U_V
function matrix_A = term_U_V(mesh, fespace)
A1 = term_P_Q(mesh, fespace);
matrix_A = blkdiag(A1, A1, A1);
end

%% term_Uold_grad_U_V
function matrix_A = term_Uold_grad_U_V(mesh, fespace, U_n)
Nb = fespace.Nb;
Nlb = fespace.Nlb;
rows = zeros(mesh.Ne*Nlb*Nlb,1);
cols = rows;
val = rows;
flag = 1;
for e = 1:mesh.Ne
    V = mesh.node(mesh.elem(e,:),:);
    [int_points_loc, int_weights_loc] = integral_loc(V, "3d");
    phi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace.basis_type, "x");
    phix = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace.basis_type, "dx");
    phiy = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace.basis_type, "dy");
    phiz = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace.basis_type, "dz");
    u1 = phi * U_n(fespace.elem(e,:));
    u2 = phi * U_n(fespace.elem(e,:)+Nb);
    u3 = phi * U_n(fespace.elem(e,:)+2*Nb);
    for i = 1:Nlb
        for j = 1:Nlb
            f = u1.*phix(:,j).*phi(:,i) + u2.*phiy(:,j).*phi(:,i) + u3.*phiz(:,j).*phi(:,i);
            r = int_weights_loc * f;
            rows(flag) = fespace.elem(e,i);
            cols(flag) = fespace.elem(e,j);
            val(flag) = r;
            flag = flag + 1;
        end
    end
end
A1 = sparse(rows, cols, val, Nb, Nb);
matrix_A = blkdiag(A1, A1, A1);
end

%% term_F_V_t
function vector_b = term_F_V_t(mesh, fespace, func_f1, func_f2, func_f3, t)
b1 = term_F_Psi_t(mesh, fespace, func_f1, t);
b2 = term_F_Psi_t(mesh, fespace, func_f2, t);
b3 = term_F_Psi_t(mesh, fespace, func_f3, t);
vector_b = [b1; b2; b3];
end

%% process_boundary_t
function [A, b] = process_boundary_t(A, b, mesh, fespace1, fespace2, pde, t)
Nb1 = fespace1.Nb;
% Neumann

% Dirichlet
for k = 1:6
    % u1
    bdn1 = bdary_node(mesh, fespace1, k);
    A(bdn1,:) = 0;
    A(sub2ind(size(A), bdn1, bdn1)) = 1;
    b(bdn1) = pde.u1(fespace1.node(bdn1,1),fespace1.node(bdn1,2),fespace1.node(bdn1,3),t);
    % u2
    bdn2 = bdn1 + Nb1;
    A(bdn2,:) = 0;
    A(sub2ind(size(A), bdn2, bdn2)) = 1;
    b(bdn2) = pde.u2(fespace1.node(bdn1,1),fespace1.node(bdn1,2),fespace1.node(bdn1,3),t);
    % u3
    bdn2 = bdn1 + 2*Nb1;
    A(bdn2,:) = 0;
    A(sub2ind(size(A), bdn2, bdn2)) = 1;
    b(bdn2) = pde.u3(fespace1.node(bdn1,1),fespace1.node(bdn1,2),fespace1.node(bdn1,3),t);
end
% p
A(3*Nb1+1,:) = 0;
A(3*Nb1+1,3*Nb1+1) = 1;
b(3*Nb1+1,1) = pde.p(pde.domain(1),pde.domain(3),pde.domain(5),t);
% M = term_P_Q(mesh, fespace2);
% matrix_A = blkdiag(sparse(3*Nb1,3*Nb1),-1e-6*M);
% A = A + matrix_A;
end

%% process_error_t
function [error_u, error_p] = process_error_t(mesh, fespace1, fespace2, pde, SOL, error_type)
Nb1 = fespace1.Nb;
t_end = pde.domain(8);
switch error_type
    case "Linf"
        err_u1 = 0;
        err_u2 = 0;
        err_u3 = 0;
        err_p = 0;
        for e = 1:mesh.Ne
            V = mesh.node(mesh.elem(e,:),:);
            [int_points_loc, ~] = integral_loc(V, "3d");
            phi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "x");
            psi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "x");
            u1h = phi * SOL(fespace1.elem(e,:));
            u2h = phi * SOL(fespace1.elem(e,:)+Nb1);
            u3h = phi * SOL(fespace1.elem(e,:)+2*Nb1);
            ph = psi * SOL(fespace2.elem(e,:)+3*Nb1);
            f1 = max(abs(pde.u1(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u1h));
            f2 = max(abs(pde.u2(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u2h));
            f3 = max(abs(pde.u3(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u3h));
            f4 = max(abs(pde.p(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - ph));
            if f1 > err_u1
                err_u1 = f1;
            end
            if f2 > err_u2
                err_u2 = f2;
            end
            if f3 > err_u3
                err_u3 = f3;
            end
            if f4 > err_p
                err_p = f4;
            end
        end
        error_u = max([err_u1, err_u2, err_u3]);
        error_p = err_p;
    case "L2"
        err_u1 = 0;
        err_u2 = 0;
        err_u3 = 0;
        err_p = 0;
        for e = 1:mesh.Ne
            V = mesh.node(mesh.elem(e,:),:);
            [int_points_loc, int_weights_loc] = integral_loc(V, "3d");
            phi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "x");
            psi = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "x");
            u1h = phi * SOL(fespace1.elem(e,:));
            u2h = phi * SOL(fespace1.elem(e,:)+Nb1);
            u3h = phi * SOL(fespace1.elem(e,:)+2*Nb1);
            ph = psi * SOL(fespace2.elem(e,:)+3*Nb1);
            f1 = (pde.u1(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u1h).^2;
            f2 = (pde.u2(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u2h).^2;
            f3 = (pde.u3(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u3h).^2;
            f4 = (pde.p(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - ph).^2;
            r1 = int_weights_loc * f1;
            r2 = int_weights_loc * f2;
            r3 = int_weights_loc * f3;
            r4 = int_weights_loc * f4;
            err_u1 = err_u1 + r1;
            err_u2 = err_u2 + r2;
            err_u3 = err_u3 + r3;
            err_p = err_p + r4;
        end
        error_u = sqrt(err_u1 + err_u2 + err_u3);
        error_p = sqrt(err_p);
    case "H1"
        err_u1x = 0;
        err_u1y = 0;
        err_u1z = 0;
        err_u2x = 0;
        err_u2y = 0;
        err_u2z = 0;
        err_u3x = 0;
        err_u3y = 0;
        err_u3z = 0;
        err_px = 0;
        err_py = 0;
        err_pz = 0;
        for e = 1:mesh.Ne
            V = mesh.node(mesh.elem(e,:),:);
            [int_points_loc, int_weights_loc] = integral_loc(V, "3d");
            phix = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dx");
            phiy = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dy");
            phiz = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace1.basis_type, "dz");
            psix = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "dx");
            psiy = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "dy");
            psiz = basis_loc(V, int_points_loc(:,1), int_points_loc(:,2), int_points_loc(:,3), fespace2.basis_type, "dz");
            u1hx = phix * SOL(fespace1.elem(e,:));
            u1hy = phiy * SOL(fespace1.elem(e,:));
            u1hz = phiz * SOL(fespace1.elem(e,:));
            u2hx = phix * SOL(fespace1.elem(e,:)+Nb1);
            u2hy = phiy * SOL(fespace1.elem(e,:)+Nb1);
            u2hz = phiz * SOL(fespace1.elem(e,:)+Nb1);
            u3hx = phix * SOL(fespace1.elem(e,:)+2*Nb1);
            u3hy = phiy * SOL(fespace1.elem(e,:)+2*Nb1);
            u3hz = phiz * SOL(fespace1.elem(e,:)+2*Nb1);
            phx = psix * SOL(fespace2.elem(e,:)+3*Nb1);
            phy = psiy * SOL(fespace2.elem(e,:)+3*Nb1);
            phz = psiz * SOL(fespace2.elem(e,:)+3*Nb1);
            f1x = (pde.u1dx(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u1hx).^2;
            f1y = (pde.u1dy(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u1hy).^2;
            f1z = (pde.u1dz(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u1hz).^2;
            f2x = (pde.u2dx(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u2hx).^2;
            f2y = (pde.u2dy(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u2hy).^2;
            f2z = (pde.u2dz(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u2hz).^2;
            f3x = (pde.u3dx(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u3hx).^2;
            f3y = (pde.u3dy(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u3hy).^2;
            f3z = (pde.u3dz(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - u3hz).^2;
            f4x = (pde.pdx(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - phx).^2;
            f4y = (pde.pdy(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - phy).^2;
            f4z = (pde.pdz(int_points_loc(:,1),int_points_loc(:,2),int_points_loc(:,3),t_end) - phz).^2;
            r1x = int_weights_loc * f1x;
            r1y = int_weights_loc * f1y;
            r1z = int_weights_loc * f1z;
            r2x = int_weights_loc * f2x;
            r2y = int_weights_loc * f2y;
            r2z = int_weights_loc * f2z;
            r3x = int_weights_loc * f3x;
            r3y = int_weights_loc * f3y;
            r3z = int_weights_loc * f3z;
            r4x = int_weights_loc * f4x;
            r4y = int_weights_loc * f4y;
            r4z = int_weights_loc * f4z;
            err_u1x = err_u1x + r1x;
            err_u1y = err_u1y + r1y;
            err_u1z = err_u1z + r1z;
            err_u2x = err_u2x + r2x;
            err_u2y = err_u2y + r2y;
            err_u2z = err_u2z + r2z;
            err_u3x = err_u3x + r3x;
            err_u3y = err_u3y + r3y;
            err_u3z = err_u3z + r3z;
            err_px = err_px + r4x;
            err_py = err_py + r4y;
            err_pz = err_pz + r4z;
        end
        error_u = sqrt(err_u1x + err_u1y + err_u1z + err_u2x + err_u2y + err_u2z + err_u3x + err_u3y + err_u3z);
        error_p = sqrt(err_px + err_py + err_pz);
    otherwise
        error("Invalid error type.");
end
end